#pragma once


#include"../include/common.hpp"
#include <cmath>
#include <fstream>
#include <iostream>


class MotionController {

public:
    struct Params {
    string username = "ricardo";
    uint16_t circles = 2;
    float kp_base = 0.0f;
    float kp_motion = 0.0f;
    float kd = 0.0f;
    float turnP = 0.0f;
    float base_speed = 0.0f;
    float kp_out_max;
    float kp_motion_out_max;
    float kd_out_max;
    
    NLOHMANN_DEFINE_TYPE_INTRUSIVE(
        Params, username, circles,kp_base, kp_motion, kd, base_speed,kp_out_max,kp_motion_out_max,kd_out_max); // 添加构造函数
    };

    Params params;
    uint16_t servoPwm = PWMSERVOMID; // 发送给舵机的PWM
    float motorSpeed = 1.0;          // 发送给电机的速度
    float kp_out = 0.0f;
    float kp_motion_out = 0.0f;
    float kd_out = 0.0f;
    void loadParams() 
    {
      string jsonPath = "../src/config/motion.json";
      std::ifstream config_is(jsonPath);
      if (!config_is.good()) {
        std::cout << "Error: Params file path:[" << jsonPath << "] not find .\n";
        exit(-1);
      }
  
      nlohmann::json js_value;
      config_is >> js_value;
  
      try {
        params = js_value.get<Params>();
      } catch (const nlohmann::detail::exception &e) {
        std::cerr << "Json Params Parse failed :" << e.what() << '\n';
        exit(-1);
      }
      motorSpeed = params.base_speed;
      cout<<"--- username:   "<<params.username<<endl;
      cout<<" |  kp_base:    "<<params.kp_base<<endl;
      cout<<" |  kp_motion:  "<<params.kp_motion<<endl;
      cout<<" |  kd:         "<<params.kd<<endl;
      cout<<" |  base_speed: "<<params.base_speed<<endl;
      cout<<" |  circles:    "<<params.circles<<endl;
    }

    void pdController(float error)
    {
        static float error_last = 0;
        if (abs(error - error_last) > COLCOMMON / 10)
        {
            error = error > error_last ? error_last + COLCOMMON / 10 : error_last - COLCOMMON / 10;
        }
        kp_motion_out = MINMAX(params.kp_motion*abs(error),-params.kp_motion_out_max,params.kp_motion_out_max);
        kp_out = MINMAX((params.kp_base+kp_motion_out)*error,-params.kp_out_max,params.kp_out_max);
        kd_out = MINMAX(params.kd * (error - error_last),-params.kd_out_max,params.kd_out_max);
        int pwmDiff = kp_out+kd_out;
        //params.turnP = abs(error) * params.kp_motion + params.kp_base;
        //int pwmDiff = (error * params.turnP) + (error - error_last) * params.kd;
        servoPwm = PWMSERVOMID + (int16_t)pwmDiff;
        error_last = error;
    }
    
    void SpeedController(float error)
    {
      float speed_k = 1.0f;
      if(my_abs(error)<=5)
      {
        speed_k = 1.15f;
      }
      else if(my_abs(error)<=10 && my_abs(error)>5)
      {
        speed_k = 1.05f;
      }
      else if(my_abs(error)<=15 && my_abs(error)>10)
      {
        speed_k = 0.97f;
      }
      else if(my_abs(error)<=20 && my_abs(error)>15)
      {
        speed_k = 0.92f;
      }
      else if(my_abs(error)<=30 && my_abs(error)>20)
      {
        speed_k = 0.85f;
      }
      else if(my_abs(error)<=40 && my_abs(error)>30)
      {
          speed_k = 0.80f;
      }
      else if(my_abs(error)<=50 && my_abs(error)>40)
      {
          speed_k = 0.75f;
      }
      else 
      {
          speed_k = 0.7f;
      }
      motorSpeed = params.base_speed * speed_k;
    }
    
    void SpeedController_rate(float rate)
    {
      
      if(rate < 0.50)
      {
        rate = 0.85;  
      }
      else if(rate > 0.5 && rate < 0.7)
      {
        rate = 0.35+rate;
      }
      else if(rate>=0.7 && rate<0.8)
      {
        rate *= 0.35+rate;
      }
      else
      {
        rate = 1.25;
      }
      motorSpeed = params.base_speed * rate;
    }
};